#include "IndoorVoxelization.h"
#include "glutesselator.h"
#include <fstream>
#include <iostream>
#include <algorithm>
#include <string>
#include "AABBTri.h"

using namespace std;

CIndoorVoxel::CIndoorVoxel(const Pointlist* in_pts, const Polygonlist* in_polys, double dVoxelSize)
{
	if (!in_pts || !in_polys || in_pts->size() < 3 || in_polys->size() == 0)
	{
		return;
	}
	//copy the data
	pts = *in_pts;
	//tessellation
	Polygonlist::const_iterator itr_poly = in_polys->begin();
	for (; itr_poly != in_polys->end(); itr_poly++)
	{
		Trianglelist PolyTris;
		if (itr_poly->ExteriorRing.size() == 3 && itr_poly->InteriorRings.size() == 0)
		{
			tris.facets.push_back(Triangle3(((*itr_poly).ExteriorRing)[0], ((*itr_poly).ExteriorRing)[1], ((*itr_poly).ExteriorRing)[2]));
		}
		else
		{
			TessellatePolygon(*itr_poly, PolyTris);
			tris.facets.reserve(tris.facets.size() + PolyTris.size());
			tris.facets.insert(tris.facets.end(), PolyTris.begin(), PolyTris.end());
		}
	}
	//voxelization
	voxelization = make_shared<CVoxelCore>(pts, dVoxelSize);
	//tag voxels
	shared_ptr<IndicesList> VoxelIndices = voxelization->QuerybySurface(tris, pts);
	IndicesList::iterator itr_voxelIndex = VoxelIndices->begin();
	for (; itr_voxelIndex != VoxelIndices->end(); itr_voxelIndex ++)
	{
		voxelization->GetVoxel(*itr_voxelIndex)->SetState(Occupied);
	}
	voxelization->TagVoxelbyFloorandCeiling(1);
}

bool CIndoorVoxel::TessellatePolygon(const Polygon3& ptrPoly, Trianglelist& PolyTris)
{
	typedef double Vec3[3];
	typedef int Tri[3];

	Tesselator tess;

	//calculate the normal
	Vec3 pNorm;
	pNorm[0] = 0.0;
	pNorm[1] = 0.0;
	pNorm[2] = 0.0;

	Vec3 lVert;
	lVert[0] = pts[*(ptrPoly.ExteriorRing.end() - 1)].x;
	lVert[1] = pts[*(ptrPoly.ExteriorRing.end() - 1)].y;
	lVert[2] = pts[*(ptrPoly.ExteriorRing.end() - 1)].z;

	vector<int>::const_iterator itrVertIndice = ptrPoly.ExteriorRing.begin();
	for (; itrVertIndice != ptrPoly.ExteriorRing.end(); itrVertIndice++)
	{
		Vec3 pStep;
		pStep[0] = (lVert[2] + pts[*itrVertIndice].z) * (lVert[1] - pts[*itrVertIndice].y);
		pStep[1] = (lVert[0] + pts[*itrVertIndice].x) * (lVert[2] - pts[*itrVertIndice].z);
		pStep[2] = (lVert[1] + pts[*itrVertIndice].y) * (lVert[0] - pts[*itrVertIndice].x);

		pNorm[0] = pNorm[0] + pStep[0];
		pNorm[1] = pNorm[1] + pStep[1];
		pNorm[2] = pNorm[2] + pStep[2];

		lVert[0] = pts[*itrVertIndice].x;
		lVert[1] = pts[*itrVertIndice].y;
		lVert[2] = pts[*itrVertIndice].z;
	}

	//do tessellation
	//verts number
	int VertNum = ptrPoly.ExteriorRing.size();
	vector<vector<int>>::const_iterator itrRing = ptrPoly.InteriorRings.begin();
	for (; itrRing != ptrPoly.InteriorRings.end(); itrRing++)
		VertNum += itrRing->size();

	tess.init(VertNum, TVec3d(pNorm[0], pNorm[1], pNorm[2]));

	std::vector<TVec3d> ring;
	std::vector<TVec2f> tag;//no use
	//add ExtierRing
	itrVertIndice = ptrPoly.ExteriorRing.begin();
	for (; itrVertIndice != ptrPoly.ExteriorRing.end(); itrVertIndice++)
	{
		TVec3d vert(pts[*itrVertIndice].x, pts[*itrVertIndice].y, pts[*itrVertIndice].z);
		ring.push_back(vert);
	}
	tess.addContour(ring, tag);
	ring.clear();
	//add InteriorRings
	itrRing = ptrPoly.InteriorRings.begin();
	for (; itrRing != ptrPoly.InteriorRings.end(); itrRing++)
	{	
		itrVertIndice = itrRing->begin();
		for (; itrVertIndice != itrRing->end(); itrVertIndice++)
		{
			TVec3d vert(pts[*itrVertIndice].x, pts[*itrVertIndice].y, pts[*itrVertIndice].z);
			ring.push_back(vert);
		}
		tess.addContour(ring, tag);
		ring.clear();
	}

	tess.compute();
	//return results
	if (tess.getIndices().size() == 0)
	{
		return false;
	}
	std::vector<unsigned int>::iterator itrTri = tess.getIndices().begin();
	for (; itrTri != tess.getIndices().end(); itrTri += 3)
	{
		//
		Triangle3 tri;
		Point3d pt1(tess.getVertices()[*itrTri].x, tess.getVertices()[*itrTri].y, tess.getVertices()[*itrTri].z);
		Point3d pt2(tess.getVertices()[*(itrTri + 1)].x, tess.getVertices()[*(itrTri + 1)].y, tess.getVertices()[*(itrTri + 1)].z);
		Point3d pt3(tess.getVertices()[*(itrTri + 2)].x, tess.getVertices()[*(itrTri + 2)].y, tess.getVertices()[*(itrTri + 2)].z);
		tri.vert[0] = std::distance(pts.begin(), std::find(pts.begin(), pts.end(), pt1));
		tri.vert[1] = std::distance(pts.begin(), std::find(pts.begin(), pts.end(), pt2));
		tri.vert[2] = std::distance(pts.begin(), std::find(pts.begin(), pts.end(), pt3));
		PolyTris.push_back(tri);
	}

	//very important
	tess.getIndices().clear();
	tess.getVertices().clear();
	tess.getTexCoords().clear();
	return true;
}

void readPoly(string Path, Pointlist& ptlist, Polygonlist& polylist)
{
	ifstream infile(Path.c_str(), ifstream::in);
	if (!infile.is_open())
	{
		return;
	}
	
	//-- read the points
	int num, tmpint;
	float tmpfloat;
	infile >> num >> tmpint >> tmpint >> tmpint;
	Pointlist::iterator itr_Point;
	//-- read first line to decide if 0- or 1-based indexing
	bool zerobased = true;
	Point3d p;
	infile >> tmpint >> p.x >> p.y >> p.z;
	//p = p/1000;
	ptlist.push_back(p);
	if (tmpint == 1)
	{
		zerobased = false;
		cout << "1-based indexing file!" << endl;
	}
	//-- process other vertices
	for (int i = 1; i < num; i++)
	{
		Point3d p;
		infile >> tmpint >> p.x >> p.y >> p.z;
		//p = p/1000;
		ptlist.push_back(p);
	}

	//-- read the facets
	infile >> num >> tmpint;
	int numf, numpt;
	string s;
	for (int i = 0; i < num; i++)
	{
		//    cout << "---- face ---- " << i << endl;
		infile >> numf >> tmpint;// >> s;
		//    cout << s << endl;
		//-- read oring (there's always one and only one)
		infile >> numpt;
		vector<int> ids(numpt);
		for (int k = 0; k < numpt; k++)
			infile >> ids[k];
		if (zerobased == false)
		{
			for (int k = 0; k < numpt; k++)
				ids[k] = (ids[k] - 1);      
		}
		Polygon3 poly;
		poly.ExteriorRing = ids;

		//-- check for irings
		for (int j = 1; j < numf; j++)
		{
			infile >> numpt;
			vector<int> ids(numpt);
			for (int l = 0; l < numpt; l++)
				infile >> ids[l];
			if (zerobased == false)
			{
				for (int k = 0; k < numpt; k++)
					ids[k] = (ids[k] - 1);      
			}

			poly.InteriorRings.push_back(ids);
		}
		//-- skip the line about point defining the hole (mandatory in a POLY file)
		if (numf > 1)
			infile >> tmpint >> tmpfloat >> tmpfloat >> tmpfloat;
		polylist.push_back(poly);
	}
}

// 
// void TestWriteVoxels(string strPath)
// {
// 	string fileName = strPath.substr(strPath.rfind("\\") + 1);
// 	ofstream fileMeta,  fileRaw;
// 	Indice3i iExtent(2, 2, 2);
// 	//
// 	fileMeta.open((strPath + string(".mhd")).c_str(), ios::out);
// 	fileMeta << "NDims = 3\n";
// 	fileMeta << "DimSize = " + to_string(static_cast<long long>(iExtent.x)) + " " + to_string(static_cast<long long>(iExtent.y)) + " " + to_string(static_cast<long long>(iExtent.z)) + "\n";
// 	fileMeta << "ElementSize = 5.0 5.0 5.0\n";
// 	fileMeta << "ElementSpacing = 5.0 5.0 5.0\n";
// 	fileMeta << "ElementType = MET_CHAR\n";
// 	fileMeta << "ElementByteOrderMSB = False\n";
// 	fileMeta << "ElementDataFile = " + fileName + string(".raw") + "\n";
// 	fileMeta.close();
// 	//
// 	char iarray[8] = {5,5,5,5,19,20,21,22};
// 	fileRaw.open((strPath + string(".raw")).c_str(), ios::binary);
// 	for (int i = 0 ; i < 8 ; i++)
// 	{
// 		fileRaw.write((char*)&iarray[i], sizeof(char));
// 	}
// 
// 	fileRaw.close();
// }

int main()
{
	//test
// 	TestWriteVoxels("C:\\Users\\John\\Documents\\test_out");
	//
// 	float cent[3];
// 	float boxhalfsize[3];
// 	float triverts[3][3];
// 	cent[0] = 0.5; 
// 	cent[1] = 0.5; 
// 	cent[2] = 0.5; 
// 	boxhalfsize[0] =  0.5;
// 	boxhalfsize[1] =  0.5;
// 	boxhalfsize[2] =  0.5;
// 	triverts[0][0] =  0.1;
// 	triverts[0][1] =  0.1;
// 	triverts[0][2] =  0.1;
// 	triverts[1][0] =  0.9;
// 	triverts[1][1] =  0.1;
// 	triverts[1][2] =  0.1;
// 	triverts[2][0] =  0.9;
// 	triverts[2][1] =  0.9;
// 	triverts[2][2] =  0.1;
// 	int result = triBoxOverlap(cent, boxhalfsize, triverts);
// 	//
	//
//  	CVoxel a;
//  	a.SetState(Occupied);
// 	int test1 = a.GetState();
// 	a.SetState(Useless);
// 	test1 = a.GetState();
// 	a.SetState(Adult2);
// 	test1 = a.GetState();
	//return 0;

	Pointlist ptlist;
	Polygonlist polylist;

	readPoly(string("C:\\Users\\John\\Documents\\indoor.poly"), ptlist, polylist);
	CIndoorVoxel test(&ptlist, &polylist, 5);
	shared_ptr<IndicesList> result = test.voxelization->QueryPathUsingAstar(Indice3i(2, 11, 1), Indice3i(24, 11, 3), Fly);
	if (result!= nullptr)
	{
		IndicesList::iterator itr = result->begin();
		for(;itr!= result->end(); itr ++)
		{
			test.voxelization->GetVoxel(*itr)->SetState(Path);
		}
	}
	test.voxelization->WriteVoxels("C:\\Users\\John\\Documents\\indoor_out");

	return 0;
}